#include "AvoidObstacles.h"
#include "AStar.h"



#include <iostream>
#include <libplayerc++/playerc++.h>
#include <stdio.h>
#include <math.h>
#include <time.h>
#include <string>

using namespace std;
using namespace PlayerCc;


//void AvoidObstacles::Follow( double xVal, double yVal)
void *Follow( void * args )
{
   time_t start_seconds, current_seconds;
   double *temp = (double*)args;
   double xVal = temp[0];
   double yVal = temp[1];
   printf("Xpath%5.3f Ypath%5.3f\n", xVal, yVal);

   //Start measuring time
   start_seconds = time(NULL);
   //DEBUGGING COMMENT
   //cout<<"START SECONDS" << start_seconds / 3600 << "\n";

   //Set up robot and proxies
   //XXX
   PlayerClient    robot("localhost"); //may want to change this
   SonarProxy      sp(&robot,0);
   Position2dProxy pp(&robot,0);

   //set up Astar and grid 
   //AStar aStar(&theGrid);
   //list<Gridcoord*> path;

   pp.SetMotorEnable(1);
   pp.SetSpeed(0.1, 0.0);
   int cycles;

   for(cycles = 0; cycles< 1000;cycles++)
   {
      double turnrate, speed;
      double desired_angle;
      double constant_angle;
      float distance = 0;

      constant_angle = 5;
      speed = 0.1;


      while(1)
      {
         // read from the proxies
         robot.Read();

         // print out sonars for fun
         cout << "SONARS: " << sp << "\n";

         current_seconds = (time(NULL) - start_seconds);
         cout << "TIME: " << current_seconds << "\n";

         //find & print distance
         distance = speed * current_seconds;
         cout << "DISTANCE: " << distance << "\n";

         if ((sp.GetScan(0) < 0.4 || 
                  sp.GetScan(1) < 0.4 ||
                  sp.GetScan(2) < 0.4 ||
                  sp.GetScan(3) < 0.49))
         {
            desired_angle = -1 * (constant_angle + 15);
            //DEBUGGING COMMENTS
            cout << "Uhoh, approaching obstacle on the left!\n";
            cout << "SONAR 0: \n" << sp.GetScan(0);
            cout << "SONAR 1: \n" << sp.GetScan(1);
            cout << "SONAR 2: \n" << sp.GetScan(2);
            cout << "SONAR 3: \n" << sp.GetScan(3);

         }//if
         else if ((sp.GetScan(4) < 0.4 || 
                  sp.GetScan(5) < 0.4 ||
                  sp.GetScan(6) < 0.4 ||
                  sp.GetScan(7) < 0.49))
         {
            desired_angle =  (constant_angle + 15);
            //DEBUGGING COMMENTS
            cout << "Uhoh, approaching obstacle on the right!\n";
            cout << "SONAR 4: \n" << sp.GetScan(4);
            cout << "SONAR 5: \n" << sp.GetScan(5);
            cout << "SONAR 6: \n" << sp.GetScan(6);
            cout << "SONAR 7: \n" << sp.GetScan(7);

         }//else if
         else
         {
            if ( xVal < (pp.GetXPos() - .3) ||
                  xVal > (pp.GetXPos() + .3) ||
                  yVal < (pp.GetYPos() - .3) ||
                  yVal > (pp.GetYPos() + .3) )
            {
               desired_angle = tan ( ( pp.GetYPos() - yVal )/ 
                     ( pp.GetXPos() - xVal ) );
            }//if
            else
            {
               desired_angle = 0;
            }//else
         }//else

         turnrate = dtor(desired_angle); // turn d_a per sec.

         pp.SetSpeed(speed, turnrate);

         cout << "SPEED: " << speed << "\n";



         printf("X %5.3f y %5.3f theta %5.3f\n",
               pp.GetXPos(),pp.GetYPos(),pp.GetYaw());

      }//while
   }//for
}//FollowWall




